﻿#region MIT
/*
Engine Of Evermore (http://code.google.com/p/engineofevermore/)
Copyright (c) 2008, 2009 Michael Woerister

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#endregion

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

using B2World = Box2DX.Dynamics.World;
using B2Vec2 = Box2DX.Common.Vec2;
using B2AABB = Box2DX.Collision.AABB;
using B2Body = Box2DX.Dynamics.Body;
using B2BodyDef = Box2DX.Dynamics.BodyDef;
using B2Shape = Box2DX.Collision.Shape;
using B2ShapeDef = Box2DX.Collision.ShapeDef;
using B2CircleDef = Box2DX.Collision.CircleDef;
using B2PolygonDef = Box2DX.Collision.PolygonDef;
using B2FilterData = Box2DX.Collision.FilterData;
using B2ContactFilter = Box2DX.Dynamics.ContactFilter;
using B2MassData = Box2DX.Collision.MassData;
using B2LineJointDef = Box2DX.Dynamics.LineJointDef;
using B2LineJoint = Box2DX.Dynamics.LineJoint;
using B2JointEdge = Box2DX.Dynamics.JointEdge;
using B2ContactListener = Box2DX.Dynamics.ContactListener;
using B2ContactPoint = Box2DX.Dynamics.ContactPoint;

using EngineOfEvermore.Common;
using Blue.ResourceManager;
using Green.Logging;
using System.Drawing;

namespace EngineOfEvermore.Scene.Physics
{
    internal sealed partial class InternalPhysicsManager : IDisposable
    {
        #region Constants
        static readonly B2AABB WORLD_BOUNDS;
        const float SHAPE_FRICTION = 0.2f;
        const float SHAPE_DENSITY = 0.0f;
        const float SHAPE_RESTITUTION = 0.0f;
        const bool SHAPE_IS_SENSOR = false;
        
        const float B2_RATIO = 0.05f;
        const float B2_INV_RATIO = 1.0f / B2_RATIO;

        const int VELOCITY_ITERATIONS = 10;
        const int POSITION_ITERATIONS = 8;
        #endregion

        #region class PhysicsHandle
        class PhysicsHandle : IPhysicsHandle
        {
            SceneObjectRef _owner;
            SortedList<int, Vector2d> _velocities = new SortedList<int, Vector2d>();
            Vector2d _velocitySum;
            
            static int _idCounter = 0;

            public PhysicsHandle( SceneObjectRef owner )
            {
                Debug.Assert( owner != null );
                _owner = owner;
            }

            public int RegisterVelocity()
            {
                int id = ++_idCounter;

                _velocities.Add( id, Vector2d.Zero );

                return id;
            }

            public void SetVelocity( int id, Vector2d v )
            {
                Debug.Assert( _velocities.ContainsKey( id ) );

                _velocities[id] = v;
                _updateVelocitySum();
            }

            public void UnregisterVelocity( int id )
            {
                Debug.Assert( _velocities.ContainsKey( id ) );
                _velocities.Remove( id );
                _updateVelocitySum();
            }

            public Vector2d VelocitySum
            {
                get { return _velocitySum; }
            }

            public int RegisterLinearConstraint( Vector2d startPoint, Vector2d endPoint )
            {
                int id = ++_idCounter;

                Vector2d axis = ( endPoint - startPoint );
                float maxTranslation = axis.Normalize() * B2_RATIO;

                B2World world = _owner.Owner._world;

                B2LineJointDef def = new B2LineJointDef();
                def.Body1 = world.GetGroundBody();
                def.Body2 = _owner.Body;
                def.CollideConnected = false;
                def.enableLimit = true;
                def.enableMotor = false;
                def.localAnchor1 = new B2Vec2( startPoint.X, startPoint.Y );
                def.localAnchor2 = new B2Vec2( startPoint.X, startPoint.Y );
                def.lowerTranslation = 0;
                def.upperTranslation = maxTranslation;
                def.localAxis1 = new B2Vec2( axis.X, axis.Y );
                
                def.UserData = id;
                
                world.CreateJoint( def );

                return id;
            }

            public void UnregisterLinearConstraint( int id )
            {
                for ( B2JointEdge je = _owner.Body.GetJointList(); je != null; je = je.Next )
                {
                    if ( je.Joint.UserData.Equals( id ) )
                    {
                        _owner.Body.GetWorld().DestroyJoint( je.Joint );
                        return;
                    }
                }
            }

            void _updateVelocitySum()
            {
                _velocitySum = Vector2d.Zero;

                foreach ( Vector2d v in _velocities.Values )
                {
                    _velocitySum += v;
                }
            }
        }
        #endregion

        #region class SceneObjectRef
        sealed class SceneObjectRef
        {
            #region Variables
            public readonly SceneObject SceneObject;
            public readonly B2Body Body;
            public readonly InternalPhysicsManager Owner;
            public readonly PhysicsHandle PhysicsHandle;
            public CollisionComponent CollisionComponent;
            #endregion

            #region Constructor
            public SceneObjectRef( SceneObject sceneObject, B2Body body,
                InternalPhysicsManager owner )
            {
                this.SceneObject = sceneObject;
                this.Body = body;
                this.Owner = owner;
                this.PhysicsHandle = new PhysicsHandle( this );
            }
            #endregion

            #region SceneObject Callbacks
            public void OnPositionChanged( SceneObject sceneObject, Vector2d pos )
            {
                this.Body.SetXForm( new B2Vec2( pos.X * B2_RATIO, pos.Y * B2_RATIO ), 
                    this.Body.GetAngle() );
            }

            public void OnLinearVelocityChanged( SceneObject sceneObject, Vector2d v )
            {
                this.Body.WakeUp();
                this.Body.SetLinearVelocity( new B2Vec2( v.X * B2_RATIO, v.Y * B2_RATIO ) );
            }

            public void OnRotationChanged( SceneObject sceneObject, float rotation )
            {
                this.Body.SetXForm( this.Body.GetPosition(), Util.DegToRad( rotation ) );
            }

            public void OnSpaceChanged( SceneObject sceneObject, int space )
            {
                Debug.Assert( space >= 0 );

                for ( B2Shape shape = this.Body.GetShapeList(); shape != null; 
                    shape = shape.GetNext() )
                {
                    var filterData = shape.FilterData;
                    filterData.GroupIndex = _spaceToFilterGroupIndex( space );
                    shape.FilterData = filterData;
                    this.Body.GetWorld().Refilter( shape );
                }
            }

            public void OnPhysicsModifierAdded( SceneObject sceneObject, IPhysicsModifier mod )
            {
                mod.OnAddToSceneObject( this.PhysicsHandle );
                this.Body.WakeUp();
            }

            public void OnPhysicsModifierRemoved( SceneObject sceneObject, IPhysicsModifier mod )
            {
                mod.OnRemoveFromSceneObject( this.PhysicsHandle );
                this.Body.WakeUp();
            }

            public void SubscribeToSceneObjectEvents()
            {
                this.SceneObject.PositionChanged += OnPositionChanged;
                this.SceneObject.RotationChanged += OnRotationChanged;
                this.SceneObject.LinearVelocityChanged += OnLinearVelocityChanged;
                this.SceneObject.SpaceChanged += OnSpaceChanged;
                this.SceneObject.PhysicsModifierAdded += OnPhysicsModifierAdded;
                this.SceneObject.PhysicsModifierRemoved += OnPhysicsModifierRemoved;
            }

            public void UnsubscribeFromSceneObjectEvents()
            {
                this.SceneObject.PositionChanged -= OnPositionChanged;
                this.SceneObject.RotationChanged -= OnRotationChanged;
                this.SceneObject.LinearVelocityChanged -= OnLinearVelocityChanged;
                this.SceneObject.SpaceChanged -= OnSpaceChanged;
                this.SceneObject.PhysicsModifierAdded -= OnPhysicsModifierAdded;
                this.SceneObject.PhysicsModifierRemoved -= OnPhysicsModifierRemoved;
            }
            #endregion

            #region CollisionComponent Callbacks
            public void OnWeightChanged( CollisionComponent component, float weight )
            {
                bool isDynamicOld = this.Body.IsDynamic();

                B2MassData massData = new B2MassData();
                massData.Mass = weight;
                this.Body.SetMass( massData );

                bool isDynamicNew = this.Body.IsDynamic();

                if ( isDynamicOld && !isDynamicNew )
                {
                    this.Owner._removeFromUpdateList( this );
                }
                else if ( !isDynamicOld && isDynamicNew )
                {
                    this.Owner._addToUpdateList( this );
                }
            }

            public void OnCollisionShapeAdded( CollisionComponent component, CollisionShape shape )
            {
                this.Owner._registerCollisionShape( component, shape );
            }

            public void OnCollisionShapeRemoved( CollisionComponent component,
                CollisionShape shape )
            {
                this.Owner._unregisterCollisionShape( component, shape );
            }

            public void OnCollisionGroupChanged( CollisionShape shape, String newGroup )
            {
                for ( B2Shape b2ShapeIt = this.Body.GetShapeList(); b2ShapeIt != null;
                      b2ShapeIt = b2ShapeIt.GetNext() )
                {
                    CollisionShape refShape = b2ShapeIt.IsSensor ?
                        ( (SensorInfo) b2ShapeIt.UserData ).Sensor.Shape :
                        ( (CollisionShapeInfo) b2ShapeIt.UserData ).CollisionShape;

                    if ( ReferenceEquals( shape, refShape ) )
                    {
                        B2FilterData newFilterData;
                        this.Owner._assignFilterSettings( out newFilterData,
                            shape,
                            this );

                        b2ShapeIt.FilterData = newFilterData;

                        this.Body.GetWorld().Refilter( b2ShapeIt );
                    }
                }
            }

            public void SubscribeToCollisionComponentEvents( CollisionComponent component )
            {
                Debug.Assert( component != null );
                Debug.Assert( this.SceneObject.GetComponent<CollisionComponent>() == component );

                component.WeightChanged += this.OnWeightChanged;
                component.CollisionShapeAdded += this.OnCollisionShapeAdded;
                component.CollisionShapeRemoved += this.OnCollisionShapeRemoved;
            }

            public void UnsubscribeFromCollisionComponentEvents( CollisionComponent component )
            {
                Debug.Assert( component != null );
                Debug.Assert( this.SceneObject.GetComponent<CollisionComponent>() == component );

                component.WeightChanged -= this.OnWeightChanged;
                component.CollisionShapeAdded -= this.OnCollisionShapeAdded;
                component.CollisionShapeRemoved -= this.OnCollisionShapeRemoved;
            }
            #endregion

            #region SensorComponent Callbacks
            public void OnSensorAdded( SensorComponent component, Sensor sensor )
            {
                this.Owner._registerSensor( component, sensor );
            }

            public void OnSensorRemoved( SensorComponent component, Sensor sensor )
            {
                this.Owner._unregisterSensor( component, sensor );
            }

            public void SubscribeToSensorComponentEvents( SensorComponent component )
            {
                Debug.Assert( component != null );
                Debug.Assert( this.SceneObject.GetComponent<SensorComponent>() == component );

                component.SensorAdded += this.OnSensorAdded;
                component.SensorRemoved += this.OnSensorRemoved;
            }

            public void UnsubscribeFromSensorComponentEvents( SensorComponent component )
            {
                Debug.Assert( component != null );
                Debug.Assert( this.SceneObject.GetComponent<SensorComponent>() == component );

                component.SensorAdded -= this.OnSensorAdded;
                component.SensorRemoved -= this.OnSensorRemoved;
            }
            #endregion
        }
        #endregion

        #region class InternalContactFilter
        class InternalContactFilter : B2ContactFilter
        {
            public override bool ShouldCollide( B2Shape shape1, B2Shape shape2 )
            {
                B2FilterData fd1 = shape1.FilterData;
                B2FilterData fd2 = shape2.FilterData;

                return ( fd1.GroupIndex == fd2.GroupIndex ) && 
                    ( (fd1.MaskBits & fd2.CategoryBits) != 0 ) && 
                    ( (fd2.MaskBits & fd1.CategoryBits) != 0 );
            }
        }
        #endregion

        #region class SensorInfo
        class SensorInfo
        {
            public SensorInfo( Sensor sensor, SceneObjectRef sceneObjectRef )
            {
                this.Sensor = sensor;
                this.SceneObjectRef = sceneObjectRef;
            }

            public readonly Sensor Sensor;
            public readonly SceneObjectRef SceneObjectRef;
        }
        #endregion

        #region class CollisionShapeInfo
        class CollisionShapeInfo    
        {
            public CollisionShapeInfo( CollisionShape collisionShape, 
                CollisionComponent collisionComponent, SceneObjectRef sceneObjectRef )
            {
                this.CollisionShape = collisionShape;
                this.CollisionComponent = collisionComponent;
                this.SceneObjectRef = sceneObjectRef;
            }

            public readonly CollisionShape CollisionShape;
            public readonly CollisionComponent CollisionComponent;
            public readonly SceneObjectRef SceneObjectRef;
        }
        #endregion

        #region class InternalContactListener
        class InternalContactListener : B2ContactListener
        {
            public override void Add( B2ContactPoint point )
            {
                if ( point.Shape1.IsSensor )
                {
                    if ( !point.Shape2.IsSensor )
                    {
                        SensorInfo sensorInfo = (SensorInfo) point.Shape1.UserData;
                        CollisionShapeInfo shapeInfo = (CollisionShapeInfo) point.Shape2.UserData;

                        sensorInfo.Sensor.AddOverlap( shapeInfo.SceneObjectRef.SceneObject );
                    }
                }
                else
                {
                    if ( point.Shape2.IsSensor )
                    {
                        SensorInfo sensorInfo = (SensorInfo) point.Shape2.UserData;
                        CollisionShapeInfo shapeInfo = (CollisionShapeInfo) point.Shape1.UserData;

                        sensorInfo.Sensor.AddOverlap( shapeInfo.SceneObjectRef.SceneObject );
                    }
                }
            }

            public override void Remove( B2ContactPoint point )
            {
                if ( point.Shape1.IsSensor )
                {
                    if ( !point.Shape2.IsSensor )
                    {
                        SensorInfo sensorInfo = (SensorInfo) point.Shape1.UserData;
                        CollisionShapeInfo shapeInfo = (CollisionShapeInfo) point.Shape2.UserData;

                        sensorInfo.Sensor.RemoveOverlap( shapeInfo.SceneObjectRef.SceneObject );
                    }
                }
                else
                {
                    if ( point.Shape2.IsSensor )
                    {
                        SensorInfo sensorInfo = (SensorInfo) point.Shape2.UserData;
                        CollisionShapeInfo shapeInfo = (CollisionShapeInfo) point.Shape1.UserData;

                        sensorInfo.Sensor.RemoveOverlap( shapeInfo.SceneObjectRef.SceneObject );
                    }
                }
            }

            public override void Result( Box2DX.Dynamics.ContactResult point )
            {
                CollisionShapeInfo si1 = (CollisionShapeInfo)point.Shape1.UserData;
                CollisionShapeInfo si2 = (CollisionShapeInfo)point.Shape2.UserData;
                SceneObjectRef ref1 = si1.SceneObjectRef;
                SceneObjectRef ref2 = si2.SceneObjectRef;

                Vector2d position = new Vector2d( point.Position.X * B2_INV_RATIO,
                    point.Position.Y * B2_INV_RATIO );

                if ( ref1.Body.IsDynamic() )
                {
                    si1.CollisionComponent.AddContact( ref2.SceneObject, position );
                }

                if ( ref2.Body.IsDynamic() )
                {
                    si2.CollisionComponent.AddContact( ref1.SceneObject, position );
                } 
            }
        }
        #endregion

        #region Variables
        bool _disposed;
        B2World _world;
        LinkedList<SceneObjectRef> _sceneObjectList;
        LinkedList<SceneObjectRef> _updateList;
        CollisionGroupSetup _collisionGroupSetup;
        #endregion

        #region Properties
        public String CollisionGroupSetup
        {
            get 
            { 
                return _collisionGroupSetup != null ? _collisionGroupSetup.ResourceName : null; 
            }
            set
            {
                if ( _collisionGroupSetup != null && _collisionGroupSetup.ResourceName == value )
                {
                    return;
                }

                if ( String.IsNullOrEmpty( value ) )
                {
                    _collisionGroupSetup = null;
                    return;
                }
                
                if ( ResourceManager.ResourceExists( value ) )
                {
                    try
                    {
                        _collisionGroupSetup = 
                            ResourceManager.GetResource<CollisionGroupSetup>( value );
                    }
                    catch ( ResourceManagerException ex )
                    {
                        Log.Error( "InternalSceneGraphPhysics.CollisionGroupSetup - Could not " + 
                                   "load CollisionGroupSetup resource " + value + ". The " + 
                                   "following error was reported: " + ex.Message );
                    }

                    B2FilterData filterData = new B2FilterData();

                    foreach ( SceneObjectRef sceneObjectRef in _sceneObjectList )
                    {
                        for ( B2Shape shapeIt = sceneObjectRef.Body.GetShapeList();
                            shapeIt != null; shapeIt = shapeIt.GetNext() )
                        {
                            _assignFilterSettings( 
                                out filterData,
                                (CollisionShape) ( shapeIt.UserData ), 
                                sceneObjectRef );

                            shapeIt.FilterData = filterData;
                            _world.Refilter( shapeIt );
                        }
                    }
                }
                else
                {
                    Log.Error( "InternalSceneGraphPhysics.CollisionGroupSetup - The " +
                               "CollisionGroupSetup resource " + value + " could not be found." );
                }
            }
        }

        #endregion

        #region Constructor, Finalizer and Static Initializer
        public InternalPhysicsManager()
        {
            _world = new B2World( WORLD_BOUNDS, B2Vec2.Zero, true );
            _world.SetContactFilter( new InternalContactFilter() );
            _world.SetContactListener( new InternalContactListener() );

            _sceneObjectList = new LinkedList<SceneObjectRef>();
            _updateList = new LinkedList<SceneObjectRef>();
        }

        ~InternalPhysicsManager()
        {
            _dispose( false );
        }

        static InternalPhysicsManager()
        {
            WORLD_BOUNDS.LowerBound = new B2Vec2( -100000.0f, -100000.0f );
            WORLD_BOUNDS.UpperBound = new B2Vec2( 100000.0f, 100000.0f );
        }
        #endregion

        #region Methods

        public void RegisterSceneObject( SceneObject sceneObject )
        {
            Debug.Assert( sceneObject != null );
            Debug.Assert( _sceneObjectList.All( x => x.SceneObject != sceneObject ) );

            // Define a body
            B2BodyDef bodyDef = new B2BodyDef();
            bodyDef.AllowSleep = true;
            bodyDef.IsSleeping = false;
            bodyDef.FixedRotation = true;
            bodyDef.IsBullet = false;
            bodyDef.Position.X = sceneObject.Position.X * B2_RATIO;
            bodyDef.Position.Y = sceneObject.Position.Y * B2_RATIO;
            bodyDef.Angle = 0.0f;
            bodyDef.MassData.Mass = 1.0f;
            
            // Create the body and assign it to the ref object
            B2Body body = _world.CreateBody( bodyDef );

            // Create a ref object
            SceneObjectRef sceneObjectRef = new SceneObjectRef( sceneObject, body, this );

            // Link the body to the sceneObjectRef
            body.SetUserData( sceneObjectRef );

            // If the scene object can move add it to the update list and register for its events
            if ( sceneObjectRef.Body.IsDynamic() )
            {
                _addToUpdateList( sceneObjectRef );
            }

            // Register physics modifiers
            foreach ( IPhysicsModifier mod in sceneObject.PhysicsModifiers )
            {
                sceneObjectRef.OnPhysicsModifierAdded( sceneObject, mod );
            }

            _sceneObjectList.AddLast( sceneObjectRef );
        }

        public void UnregisterSceneObject( SceneObject sceneObject )
        {
            Debug.Assert( sceneObject != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == sceneObject ) == 1 );

            LinkedListNode<SceneObjectRef> it = _sceneObjectList.First;

            for ( ; it != null; it = it.Next )
            {
                if ( it.Value.SceneObject == sceneObject )
                {
                    break;
                }
            }

            SceneObjectRef sceneObjectRef = it.Value;

            _sceneObjectList.Remove( it );

            // Unregister physics modifiers
            foreach ( IPhysicsModifier mod in sceneObject.PhysicsModifiers )
            {
                sceneObjectRef.OnPhysicsModifierRemoved( sceneObject, mod );
            }

            if ( sceneObjectRef.Body.IsDynamic() )
            {
                _removeFromUpdateList( sceneObjectRef );
            }

            _world.DestroyBody( sceneObjectRef.Body );
        }

        public void RegisterCollisionComponent( CollisionComponent component )
        {
            Debug.Assert( component != null );
            Debug.Assert( component.Owner != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == component.Owner ) == 1 );

            SceneObjectRef sceneObjectRef =
                _getSceneObjectRefFor( component.Owner );

            Debug.Assert( sceneObjectRef.CollisionComponent == null );

            sceneObjectRef.CollisionComponent = component;

            foreach ( CollisionShape shape in component.Shapes )
            {
                this._registerCollisionShape( component, shape );
            }

            sceneObjectRef.SubscribeToCollisionComponentEvents( component );

            B2MassData massData = new B2MassData();
            massData.Mass = component.Weight;
            sceneObjectRef.Body.SetMass( massData );
        }

        public void UnregisterCollisionComponent( CollisionComponent component )
        {
            Debug.Assert( component != null );
            Debug.Assert( component.Owner != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == component.Owner ) == 1 );

            SceneObjectRef sceneObjectRef =
                _getSceneObjectRefFor( component.Owner );

            Debug.Assert(
                sceneObjectRef.SceneObject.GetComponent<CollisionComponent>() == component );

            B2MassData massData = new B2MassData();
            massData.Mass = 1.0f;
            sceneObjectRef.Body.SetMass( massData );

            sceneObjectRef.UnsubscribeFromCollisionComponentEvents( component );

            foreach ( CollisionShape shape in component.Shapes )
            {
                this._unregisterCollisionShape( component, shape );
            }

            Debug.Assert( sceneObjectRef.CollisionComponent == component );

            sceneObjectRef.CollisionComponent = null;
        }

        public void RegisterSensorComponent( SensorComponent component )
        {
            Debug.Assert( component != null );
            Debug.Assert( component.Owner != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == component.Owner ) == 1 );

            SceneObjectRef sceneObjectRef =
                _getSceneObjectRefFor( component.Owner );

            foreach ( Sensor sensor in component.Sensors )
            {
                _registerSensor( component, sensor );
            }

            sceneObjectRef.SubscribeToSensorComponentEvents( component );
        }

        public void UnregisterSensorComponent( SensorComponent component )
        {
            Debug.Assert( component != null );
            Debug.Assert( component.Owner != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == component.Owner ) == 1 );

            SceneObjectRef sceneObjectRef =
                _getSceneObjectRefFor( component.Owner );

            sceneObjectRef.UnsubscribeFromSensorComponentEvents( component );

            foreach ( Sensor sensor in component.Sensors )
            {
                _unregisterSensor( component, sensor );
            }
        }

        public void Update( LogicUpdateEventArgs e )
        {
            foreach ( SceneObjectRef sceneObjectRef in _updateList )
            {
                Vector2d targetVelocity = sceneObjectRef.SceneObject.LinearVelocity * B2_RATIO +
                    sceneObjectRef.PhysicsHandle.VelocitySum * B2_RATIO;

                B2Vec2 actualVelocity = sceneObjectRef.Body.GetLinearVelocity();

                if ( !targetVelocity.X.EpsilonEquals( actualVelocity.X ) ||
                     !targetVelocity.Y.EpsilonEquals( actualVelocity.Y ) )
                {
                    sceneObjectRef.Body.SetLinearVelocity( 
                        new B2Vec2( targetVelocity.X, targetVelocity.Y ) );
                }

                if ( sceneObjectRef.CollisionComponent != null )
                {
                    sceneObjectRef.CollisionComponent.ClearContacts();
                }
            }

            _world.Step( e.ElapsedTime, VELOCITY_ITERATIONS, POSITION_ITERATIONS );

            foreach ( SceneObjectRef sceneObjectRef in _updateList )
            {
                if ( sceneObjectRef.Body.IsSleeping() )
                {
                    continue;
                }

                B2Vec2 b2Position = sceneObjectRef.Body.GetPosition();
                
                sceneObjectRef.SceneObject.UpdateXForm( b2Position.X * B2_INV_RATIO,
                    b2Position.Y * B2_INV_RATIO, Util.RadToDeg( sceneObjectRef.Body.GetAngle() ) );
            }
        }

        public void Dispose()
        {
            _dispose( true );
            GC.SuppressFinalize( this );
        }
        #endregion

        #region Private Methods
        void _dispose( bool disposing )
        {
            if ( !_disposed )
            {
                if ( disposing )
                {
                    if ( _world != null )
                    {
                        _world.Dispose();
                        _world = null;
                    }
                }

                _disposed = true;
            }
        }

        SceneObjectRef _getSceneObjectRefFor( SceneObject sceneObject )
        {
            LinkedListNode<SceneObjectRef> it = _sceneObjectList.First;

            for ( ; it != null; it = it.Next )
            {
                if ( it.Value.SceneObject == sceneObject )
                {
                    return it.Value;
                }
            }

            return null;
        }

        void _addToUpdateList( SceneObjectRef sceneObjectRef )
        {
            Debug.Assert( !_updateList.Contains( sceneObjectRef ) );
            _updateList.AddLast( sceneObjectRef );

            sceneObjectRef.SubscribeToSceneObjectEvents();
        }

        void _removeFromUpdateList( SceneObjectRef sceneObjectRef )
        {
            Debug.Assert( _updateList.Contains( sceneObjectRef ) );

            sceneObjectRef.UnsubscribeFromSceneObjectEvents();

            _updateList.Remove( sceneObjectRef );
        }

        void _registerCollisionShape( CollisionComponent component, CollisionShape shape )
        {
            Debug.Assert( component.Owner != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == component.Owner ) == 1 );

            SceneObjectRef sceneObjectRef =
                _getSceneObjectRefFor( component.Owner );

            shape.CollisionGroupChanged += sceneObjectRef.OnCollisionGroupChanged;

            IEnumerable<B2ShapeDef> shapeDefs;

            switch ( shape.ShapeType )
            {
                case CollisionShapeType.Circle:
                    shapeDefs = _createDefForShape( sceneObjectRef, (CircleShape) shape );
                    break;

                case CollisionShapeType.Polygon:
                    shapeDefs = _createDefForShape( sceneObjectRef, (PolygonShape) shape );
                    break;

                case CollisionShapeType.Rectangle:
                    shapeDefs = _createDefForShape( sceneObjectRef, (RectangleShape) shape );
                    break;

                case CollisionShapeType.RoundedRectangle:
                    shapeDefs = _createDefForShape( sceneObjectRef, 
                        (RoundedRectangleShape) shape );
                    break;

                default:
                    shapeDefs = null;
                    break;
            }

            Debug.Assert( shapeDefs != null );

            foreach ( B2ShapeDef def in shapeDefs )
            {
                def.UserData = new CollisionShapeInfo( shape, component, sceneObjectRef );
                def.IsSensor = false;
                sceneObjectRef.Body.CreateShape( def );
            }
        }

        void _unregisterCollisionShape( CollisionComponent component, CollisionShape shape )
        {
            Debug.Assert( component.Owner != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == component.Owner ) == 1 );

            SceneObjectRef sceneObjectRef =
                _getSceneObjectRefFor( component.Owner );

            B2Body body = sceneObjectRef.Body;

            B2Shape b2ShapeIt = body.GetShapeList();

            while ( b2ShapeIt != null )
            {
                if ( !b2ShapeIt.IsSensor && 
                    ((CollisionShapeInfo)b2ShapeIt.UserData).CollisionShape == shape )
                {
                    B2Shape shapeToDelete = b2ShapeIt;
                    b2ShapeIt = b2ShapeIt.GetNext();

                    body.DestroyShape( shapeToDelete );
                }
                else
                {
                    b2ShapeIt = b2ShapeIt.GetNext();
                }
            }

            shape.CollisionGroupChanged -= sceneObjectRef.OnCollisionGroupChanged;
        }

        void _registerSensor( SensorComponent component, Sensor sensor )
        {
            Debug.Assert( component.Owner != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == component.Owner ) == 1 );

            SceneObjectRef sceneObjectRef =
                _getSceneObjectRefFor( component.Owner );

            sensor.Shape.CollisionGroupChanged += sceneObjectRef.OnCollisionGroupChanged;

            IEnumerable<B2ShapeDef> shapeDefs;

            switch ( sensor.Shape.ShapeType )
            {
                case CollisionShapeType.Circle:
                    shapeDefs = _createDefForShape( sceneObjectRef, (CircleShape) sensor.Shape );
                    break;

                case CollisionShapeType.Polygon:
                    shapeDefs = _createDefForShape( sceneObjectRef, (PolygonShape) sensor.Shape );
                    break;

                case CollisionShapeType.Rectangle:
                    shapeDefs = _createDefForShape( sceneObjectRef, 
                        (RectangleShape) sensor.Shape );
                    break;

                case CollisionShapeType.RoundedRectangle:
                    shapeDefs = _createDefForShape( sceneObjectRef, 
                        (RoundedRectangleShape) sensor.Shape );
                    break;

                default:
                    shapeDefs = null;
                    break;
            }

            Debug.Assert( shapeDefs != null );

            foreach ( B2ShapeDef def in shapeDefs )
            {
                def.UserData = new SensorInfo( sensor, sceneObjectRef );
                def.IsSensor = true;
                sceneObjectRef.Body.CreateShape( def );
            }
        }

        void _unregisterSensor( SensorComponent component, Sensor sensor )
        {
            Debug.Assert( component.Owner != null );
            Debug.Assert( _sceneObjectList.Count( x => x.SceneObject == component.Owner ) == 1 );

            SceneObjectRef sceneObjectRef =
                _getSceneObjectRefFor( component.Owner );

            B2Body body = sceneObjectRef.Body;

            B2Shape b2ShapeIt = body.GetShapeList();

            while ( b2ShapeIt != null )
            {
                if ( b2ShapeIt.IsSensor &&
                    ( (SensorInfo) b2ShapeIt.UserData ).Sensor == sensor )
                {
                    B2Shape shapeToDelete = b2ShapeIt;
                    b2ShapeIt = b2ShapeIt.GetNext();

                    body.DestroyShape( shapeToDelete );
                }
                else
                {
                    b2ShapeIt = b2ShapeIt.GetNext();
                }
            }

            sensor.Shape.CollisionGroupChanged -= sceneObjectRef.OnCollisionGroupChanged;
            sensor.ClearOverlaps();
        }

        static void _assignDefaultValues( B2ShapeDef def, CollisionShape shape, 
            SceneObjectRef sceneObjectRef )
        {
            def.Friction = SHAPE_FRICTION;
            def.Restitution = SHAPE_RESTITUTION;
            def.Density = SHAPE_DENSITY;
        }

        static short _spaceToFilterGroupIndex( int space )
        {
            return (short) space;
        }

        void _assignFilterSettings( out B2FilterData filterData,
                                    CollisionShape shape, 
                                    SceneObjectRef sceneObjectRef 
                                    )
        { 
            if ( shape.CollisionGroup != null && _collisionGroupSetup != null )
            {
                Debug.Assert( _collisionGroupSetup.ContainsGroup( shape.CollisionGroup ) );

                _collisionGroupSetup.GetMasksForGroup(
                    shape.CollisionGroup,
                    out filterData.MaskBits,
                    out filterData.CategoryBits 
                    );

                filterData.GroupIndex = 
                    _spaceToFilterGroupIndex( sceneObjectRef.SceneObject.Space );
            }
            else
            {
                filterData.CategoryBits = 0;
                filterData.MaskBits = 0;
                filterData.GroupIndex = short.MinValue;
            }
        }

        IEnumerable<B2ShapeDef> _createDefForShape( SceneObjectRef sceneObjectRef, 
            CircleShape shape )
        {
            B2CircleDef def = new B2CircleDef();
            _assignDefaultValues( def, shape, sceneObjectRef );
            _assignFilterSettings( out def.Filter, shape, sceneObjectRef );

            def.LocalPosition = new B2Vec2( shape.Offset.X * B2_RATIO, shape.Offset.Y * B2_RATIO );
            def.Radius = shape.Radius * B2_RATIO;
            yield return def;
        }

        IEnumerable<B2ShapeDef> _createDefForShape( SceneObjectRef sceneObjectRef, 
            PolygonShape shape )
        {
            B2PolygonDef def = new B2PolygonDef();
            _assignDefaultValues( def, shape, sceneObjectRef );
            _assignFilterSettings( out def.Filter, shape, sceneObjectRef );

            int vertexCount = 0;

            foreach ( Vector2d vertex in shape.Vertices )
            {
                def.Vertices[vertexCount] = new B2Vec2( vertex.X * B2_RATIO, vertex.Y * B2_RATIO );
                ++vertexCount;
            }

            def.VertexCount = vertexCount;
            yield return def;
        }

        IEnumerable<B2ShapeDef> _createDefForShape( SceneObjectRef sceneObjectRef, 
            RectangleShape shape )
        {
            B2PolygonDef def = new B2PolygonDef();
            _assignDefaultValues( def, shape, sceneObjectRef );
            _assignFilterSettings( out def.Filter, shape, sceneObjectRef );

            def.Vertices[0].X = shape.Rectangle.Left * B2_RATIO;
            def.Vertices[0].Y = shape.Rectangle.Top * B2_RATIO;

            def.Vertices[1].X = shape.Rectangle.Right * B2_RATIO;
            def.Vertices[1].Y = shape.Rectangle.Top * B2_RATIO;

            def.Vertices[2].X = shape.Rectangle.Right * B2_RATIO;
            def.Vertices[2].Y = shape.Rectangle.Bottom * B2_RATIO;

            def.Vertices[3].X = shape.Rectangle.Left * B2_RATIO;
            def.Vertices[3].Y = shape.Rectangle.Bottom * B2_RATIO;

            def.VertexCount = 4;
            yield return def;
        }


        IEnumerable<B2ShapeDef> _createDefForShape( SceneObjectRef sceneObjectRef,
            RoundedRectangleShape shape )
        {
            B2PolygonDef polyDef = new B2PolygonDef();
            _assignDefaultValues( polyDef, shape, sceneObjectRef );
            _assignFilterSettings( out polyDef.Filter, shape, sceneObjectRef );

            polyDef.VertexCount = 8;

            polyDef.Vertices[0].X = ( shape.Rectangle.Left + shape.CornerRadius ) * B2_RATIO;
            polyDef.Vertices[0].Y = shape.Rectangle.Top * B2_RATIO;

            polyDef.Vertices[1].X = ( shape.Rectangle.Right - shape.CornerRadius ) * B2_RATIO;
            polyDef.Vertices[1].Y = shape.Rectangle.Top * B2_RATIO;

            polyDef.Vertices[2].X = shape.Rectangle.Right * B2_RATIO;
            polyDef.Vertices[2].Y = ( shape.Rectangle.Top + shape.CornerRadius ) * B2_RATIO;

            polyDef.Vertices[3].X = shape.Rectangle.Right * B2_RATIO;
            polyDef.Vertices[3].Y = ( shape.Rectangle.Bottom - shape.CornerRadius ) * B2_RATIO;

            polyDef.Vertices[4].X = ( shape.Rectangle.Right - shape.CornerRadius ) * B2_RATIO;
            polyDef.Vertices[4].Y = shape.Rectangle.Bottom * B2_RATIO;

            polyDef.Vertices[5].X = ( shape.Rectangle.Left + shape.CornerRadius ) * B2_RATIO;
            polyDef.Vertices[5].Y = shape.Rectangle.Bottom * B2_RATIO;

            polyDef.Vertices[6].X = shape.Rectangle.Left * B2_RATIO;
            polyDef.Vertices[6].Y = ( shape.Rectangle.Bottom - shape.CornerRadius ) * B2_RATIO;

            polyDef.Vertices[7].X = shape.Rectangle.Left * B2_RATIO;
            polyDef.Vertices[7].Y = ( shape.Rectangle.Top + shape.CornerRadius ) * B2_RATIO;

            yield return polyDef;

            // Upper left circle
            B2CircleDef circleDef = new B2CircleDef();
            _assignDefaultValues( circleDef, shape, sceneObjectRef );
            _assignFilterSettings( out circleDef.Filter, shape, sceneObjectRef );

            circleDef.Radius = shape.CornerRadius * B2_RATIO;
            circleDef.LocalPosition.X = ( shape.Rectangle.Left + shape.CornerRadius ) * B2_RATIO;
            circleDef.LocalPosition.Y = ( shape.Rectangle.Top + shape.CornerRadius ) * B2_RATIO;
            yield return circleDef;

            // Upper right circle
            circleDef.LocalPosition.X = ( shape.Rectangle.Right - shape.CornerRadius ) * B2_RATIO;
            yield return circleDef;

            // Lower right circle
            circleDef.LocalPosition.Y = ( shape.Rectangle.Bottom - shape.CornerRadius ) * B2_RATIO;
            yield return circleDef;

            // Lower left circle
            circleDef.LocalPosition.X = ( shape.Rectangle.Left + shape.CornerRadius ) * B2_RATIO;
            yield return circleDef;
        }
        #endregion
    }
}
